﻿using System;

namespace Silver3D.Math
{
    public struct Matrix44
    {
        public static Matrix44 Zero { get { return new Matrix44(); } }
        public static Matrix44 Identity { get { return new Matrix44(1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1); } }

        private double[] m;

        public double M11 { get { return m[0]; } set { m[0] = value; } }
        public double M12 { get { return m[1]; } set { m[1] = value; } }
        public double M13 { get { return m[2]; } set { m[2] = value; } }
        public double M14 { get { return m[3]; } set { m[3] = value; } }
        public double TX { get { return m[3]; } set { m[3] = value; } }

        public double M21 { get { return m[4]; } set { m[4] = value; } }
        public double M22 { get { return m[5]; } set { m[5] = value; } }
        public double M23 { get { return m[6]; } set { m[6] = value; } }
        public double M24 { get { return m[7]; } set { m[7] = value; } }
        public double TY { get { return m[7]; } set { m[7] = value; } }

        public double M31 { get { return m[8]; } set { m[8] = value; } }
        public double M32 { get { return m[9]; } set { m[9] = value; } }
        public double M33 { get { return m[10]; } set { m[10] = value; } }
        public double M34 { get { return m[11]; } set { m[11] = value; } }
        public double TZ { get { return m[11]; } set { m[11] = value; } }

        public double M41 { get { return m[12]; } set { m[12] = value; } }
        public double M42 { get { return m[13]; } set { m[13] = value; } }
        public double M43 { get { return m[14]; } set { m[14] = value; } }
        public double M44 { get { return m[15]; } set { m[15] = value; } }

        public Matrix44 Transpose
        {
            get { return new Matrix44(m[0], m[4], m[8], m[12], m[1], m[5], m[9], m[13], m[2], m[6], m[10], m[14], m[3], m[7], m[11], m[15]); }
        }

        static int[] sel1 = new int[] { 1, 0, 0, 0 };
        static int[] sel2 = new int[] { 2, 2, 1, 1 };
        static int[] sel3 = new int[] { 3, 3, 3, 2 };

        private double GetCofactorElement(int row, int col)
        {
            // cofactor element is the signed minor (the signed determinant of the remaining matrix)
            int row1 = sel1[row];
            int row2 = sel2[row];
            int row3 = sel3[row];
            int col1 = sel1[col];
            int col2 = sel2[col];
            int col3 = sel3[col];

            double det =
                (this[row1, col1] * this[row2, col2] * this[row3, col3]) +
                (this[row1, col2] * this[row2, col3] * this[row3, col1]) +
                (this[row1, col3] * this[row2, col1] * this[row3, col2]) -
                (this[row1, col1] * this[row2, col3] * this[row3, col2]) -
                (this[row1, col2] * this[row2, col1] * this[row3, col3]) -
                (this[row1, col3] * this[row2, col2] * this[row3, col1]);

            // negative if (row + col) is odd.
            bool flipSign = ((row + col) & 0x1) == 1;

            // any way to remove this check in safe C#?
            return flipSign ? -det : det;
        }

        public Matrix44 CofactorMatrix
        {
            get
            {
                Matrix44 mat = Identity;
                int r = 0, c = 0;
                for (r = 0; r < 4; ++r)
                    for (c = 0; c < 4; ++c)
                        mat[r, c] = GetCofactorElement(r, c);
                return mat;
            }
        }

        private double GetDeterminant(ref Matrix44 cof)
        {
            double det = 0.0;
            for (int r = 0; r < 4; ++r) det += this[r, 0] * cof[r, 0];
            return det;
        }

        public double Determinant
        {
            get
            {
                Matrix44 cof = CofactorMatrix;
                return GetDeterminant(ref cof);
            }
        }

        public bool IsInvertible
        {
            get { return !Util.IsEquivalent(0.0, Determinant); }
        }

        public Matrix44 Inverse
        {
            get
            {
                Matrix44 cof = CofactorMatrix;
                double det = GetDeterminant(ref cof);

                if (det == 0.0)
                    throw new InvalidOperationException("Cannot inverse a singular matrix.");

                return (1.0 / det) * cof.Transpose;
            }
        }

        public bool IsIdentity
        {
            get { return this == Identity; }
        }

        public Matrix44(
            double m11, double m12, double m13, double m14,
            double m21, double m22, double m23, double m24,
            double m31, double m32, double m33, double m34,
            double m41, double m42, double m43, double m44)
        {
            this.m = new double[16];
            this.m[0] = m11;
            this.m[1] = m12;
            this.m[2] = m13;
            this.m[3] = m14;

            this.m[4] = m21;
            this.m[5] = m22;
            this.m[6] = m23;
            this.m[7] = m24;

            this.m[8] = m31;
            this.m[9] = m32;
            this.m[10] = m33;
            this.m[11] = m34;

            this.m[12] = m41;
            this.m[13] = m42;
            this.m[14] = m43;
            this.m[15] = m44;
        }

        public Matrix44(Vec4 row1, Vec4 row2, Vec4 row3, Vec4 row4)
            : this(row1.X, row1.Y, row1.Z, row1.W,
            row2.X, row2.Y, row2.Z, row2.W,
            row3.X, row3.Y, row3.Z, row3.W,
            row4.X, row4.Y, row4.Z, row4.W)
        {
        }

        public Matrix44(Matrix44 copy)
        {
            this.m = new double[16];
            Array.Copy(copy.m, this.m, 16);
        }

        public Matrix44(Matrix33 rotScale, Vec3 translation)
            : this(rotScale.M11, rotScale.M12, rotScale.M13, translation.X,
                rotScale.M21, rotScale.M22, rotScale.M23, translation.Y,
                rotScale.M31, rotScale.M32, rotScale.M33, translation.Z,
                0.0, 0.0, 0.0, 1.0)
        {
        }

        public double this[int row, int col]
        {
            get
            {
                return m[(row << 2) + col];
            }
            set
            {
                m[(row << 2) + col] = value;
            }
        }

        public static Matrix44 operator -(Matrix44 b)
        {
            return new Matrix44(
                -b.m[0], -b.m[1], -b.m[2], -b.m[3],
                -b.m[4], -b.m[5], -b.m[6], -b.m[7],
                -b.m[8], -b.m[9], -b.m[10], -b.m[11],
                -b.m[12], -b.m[13], -b.m[14], -b.m[15]);
        }

        public static Matrix44 operator -(Matrix44 a, Matrix44 b)
        {
            return new Matrix44(
                a.m[0] - b.m[0], a.m[1] - b.m[1], a.m[2] - b.m[2], a.m[3] - b.m[3],
                a.m[4] - b.m[4], a.m[5] - b.m[5], a.m[6] - b.m[6], a.m[7] - b.m[7],
                a.m[8] - b.m[8], a.m[9] - b.m[9], a.m[10] - b.m[10], a.m[11] - b.m[11],
                a.m[12] - b.m[12], a.m[13] - b.m[13], a.m[14] - b.m[14], a.m[15] - b.m[15]);
        }

        public static Matrix44 operator +(Matrix44 a, Matrix44 b)
        {
            return new Matrix44(
                a.m[0] + b.m[0], a.m[1] + b.m[1], a.m[2] + b.m[2], a.m[3] + b.m[3],
                a.m[4] + b.m[4], a.m[5] + b.m[5], a.m[6] + b.m[6], a.m[7] + b.m[7],
                a.m[8] + b.m[8], a.m[9] + b.m[9], a.m[10] + b.m[10], a.m[11] + b.m[11],
                a.m[12] + b.m[12], a.m[13] + b.m[13], a.m[14] + b.m[14], a.m[15] + b.m[15]);
        }

        public static Matrix44 operator *(Matrix44 a, double s)
        {
            return new Matrix44(
                a.m[0] * s, a.m[1] * s, a.m[2] * s, a.m[3] * s,
                a.m[4] * s, a.m[5] * s, a.m[6] * s, a.m[7] * s,
                a.m[8] * s, a.m[9] * s, a.m[10] * s, a.m[11] * s,
                a.m[12] * s, a.m[13] * s, a.m[14] * s, a.m[15] * s);
        }

        public static Matrix44 operator *(double s, Matrix44 a)
        {
            return a * s;
        }

        public static Matrix44 operator /(Matrix44 a, double s)
        {
            double sInv = 1.0 / s;
            return sInv * a;
        }

        public static Matrix44 operator /(double s, Matrix44 a)
        {
            return a / s;
        }

        public static Matrix44 operator *(Matrix44 a, Matrix44 b)
        {
            return new Matrix44(
               a.m[0] * b.m[0] + a.m[1] * b.m[4] + a.m[2] * b.m[8] + a.m[3] * b.m[12],
               a.m[0] * b.m[1] + a.m[1] * b.m[5] + a.m[2] * b.m[9] + a.m[3] * b.m[13],
               a.m[0] * b.m[2] + a.m[1] * b.m[6] + a.m[2] * b.m[10] + a.m[3] * b.m[14],
               a.m[0] * b.m[3] + a.m[1] * b.m[7] + a.m[2] * b.m[11] + a.m[3] * b.m[15],

               a.m[4] * b.m[0] + a.m[5] * b.m[4] + a.m[6] * b.m[8] + a.m[7] * b.m[12],
               a.m[4] * b.m[1] + a.m[5] * b.m[5] + a.m[6] * b.m[9] + a.m[7] * b.m[13],
               a.m[4] * b.m[2] + a.m[5] * b.m[6] + a.m[6] * b.m[10] + a.m[7] * b.m[14],
               a.m[4] * b.m[3] + a.m[5] * b.m[7] + a.m[6] * b.m[11] + a.m[7] * b.m[15],

               a.m[8] * b.m[0] + a.m[9] * b.m[4] + a.m[10] * b.m[8] + a.m[11] * b.m[12],
               a.m[8] * b.m[1] + a.m[9] * b.m[5] + a.m[10] * b.m[9] + a.m[11] * b.m[13],
               a.m[8] * b.m[2] + a.m[9] * b.m[6] + a.m[10] * b.m[10] + a.m[11] * b.m[14],
               a.m[8] * b.m[3] + a.m[9] * b.m[7] + a.m[10] * b.m[11] + a.m[11] * b.m[15],

               a.m[12] * b.m[0] + a.m[13] * b.m[4] + a.m[14] * b.m[8] + a.m[15] * b.m[12],
               a.m[12] * b.m[1] + a.m[13] * b.m[5] + a.m[14] * b.m[9] + a.m[15] * b.m[13],
               a.m[12] * b.m[2] + a.m[13] * b.m[6] + a.m[14] * b.m[10] + a.m[15] * b.m[14],
               a.m[12] * b.m[3] + a.m[13] * b.m[7] + a.m[14] * b.m[11] + a.m[15] * b.m[15]);
        }

        public static Vec4 operator *(Matrix44 a, Vec4 b)
        {
            return new Vec4(
                a.m[0] * b.X + a.m[1] * b.Y + a.m[2] * b.Z + a.m[3] * b.W,
                a.m[4] * b.X + a.m[5] * b.Y + a.m[6] * b.Z + a.m[7] * b.W,
                a.m[8] * b.X + a.m[9] * b.Y + a.m[10] * b.Z + a.m[11] * b.W,
                a.m[12] * b.X + a.m[13] * b.Y + a.m[14] * b.Z + a.m[15] * b.W);
        }

        public static bool operator ==(Matrix44 a, Matrix44 b)
        {
            return
                a.m[0] == b.m[0] &&
                a.m[1] == b.m[1] &&
                a.m[2] == b.m[2] &&
                a.m[3] == b.m[3] &&

                a.m[4] == b.m[4] &&
                a.m[5] == b.m[5] &&
                a.m[6] == b.m[6] &&
                a.m[7] == b.m[7] &&

                a.m[8] == b.m[8] &&
                a.m[9] == b.m[9] &&
                a.m[10] == b.m[10] &&
                a.m[11] == b.m[11] &&

                a.m[12] == b.m[12] &&
                a.m[13] == b.m[13] &&
                a.m[14] == b.m[14] &&
                a.m[15] == b.m[15];
        }

        public static bool operator !=(Matrix44 a, Matrix44 b)
        {
            return !(a == b);
        }

        public Vec3 TransformPoint(Vec3 p)
        {
            Vec4 v = this * (new Vec4(p.X, p.Y, p.Z, 1.0));
            return v.ToVec3();
        }

        public Vec3 TransformVector(Vec3 v)
        {
            Vec4 vec = this * (new Vec4(v.X, v.Y, 0.0));
            return new Vec3(vec);
        }

        public void Invert()
        {
            this = Inverse;
        }

        public override string ToString()
        {
            return string.Format("Matrix44(({0},{1},{2},{3}),({4},{5},{6},{7}),({8},{9},{10},{11}),({12},{13},{14},{15}))",
                m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8], m[9], m[10], m[11], m[12], m[13], m[14], m[15]);
        }

        public bool IsEquivalent(Matrix44 b)
        {
            if (m == null) return false;
            return
                Util.IsEquivalent(m[0], b.m[0]) && Util.IsEquivalent(m[1], b.m[1]) && Util.IsEquivalent(m[2], b.m[2]) && Util.IsEquivalent(m[3], b.m[3]) &&
                Util.IsEquivalent(m[4], b.m[4]) && Util.IsEquivalent(m[5], b.m[5]) && Util.IsEquivalent(m[6], b.m[6]) && Util.IsEquivalent(m[7], b.m[7]) &&
                Util.IsEquivalent(m[8], b.m[8]) && Util.IsEquivalent(m[9], b.m[9]) && Util.IsEquivalent(m[10], b.m[10]) && Util.IsEquivalent(m[11], b.m[11]) &&
                Util.IsEquivalent(m[12], b.m[12]) && Util.IsEquivalent(m[13], b.m[13]) && Util.IsEquivalent(m[14], b.m[14]) && Util.IsEquivalent(m[15], b.m[15]);
        }

        public override bool Equals(object obj)
        {
            return (obj != null) && (obj is Matrix44) && ((Matrix44)obj) == this;
        }

        public override int GetHashCode()
        {
            return
                m[0].GetHashCode() ^ m[1].GetHashCode() ^ m[2].GetHashCode() ^ m[3].GetHashCode() ^
                m[4].GetHashCode() ^ m[5].GetHashCode() ^ m[6].GetHashCode() ^ m[7].GetHashCode() ^
                m[8].GetHashCode() ^ m[9].GetHashCode() ^ m[10].GetHashCode() ^ m[11].GetHashCode() ^
                m[12].GetHashCode() ^ m[13].GetHashCode() ^ m[14].GetHashCode() ^ m[15].GetHashCode();
        }

        public static Matrix44 CreateTranslation(Vec3 v)
        {
            return CreateTranslation(v.X, v.Y, v.Z);
        }

        public static Matrix44 CreateTranslation(double tx, double ty, double tz)
        {
            return new Matrix44(1, 0, 0, tx, 0, 1, 0, ty, 0, 0, 1, tz, 0, 0, 0, 1);
        }

        public static Matrix44 CreateScale(Vec3 s)
        {
            return CreateScale(s.X, s.Y, s.Z);
        }

        public static Matrix44 CreateScale(double sx, double sy, double sz)
        {
            return new Matrix44(sx, 0, 0, 0, 0, sy, 0, 0, 0, 0, sz, 0, 0, 0, 0, 1);
        }

        public static Matrix44 CreateScale(double s)
        {
            return CreateScale(s, s, s);
        }

        public static Matrix44 CreateAngleAxis(double angle, Vec3 axis)
        {
            Vec3 aSq = axis * axis;
            double cosT = System.Math.Cos(angle);
            double OneMinCosT = 1.0 - cosT;
            double sinT = System.Math.Sin(angle);

            return new Matrix44(
                aSq.X + (aSq.Y + aSq.Z) * cosT,
                axis.X * axis.Y * OneMinCosT - axis.Z * sinT,
                axis.X * axis.Z * OneMinCosT + axis.Y * sinT,
                0.0,
                axis.X * axis.Y * OneMinCosT + axis.Z * sinT,
                aSq.Y + (aSq.X + aSq.Z) * cosT,
                axis.Y * axis.Z * OneMinCosT - axis.X * sinT,
                0.0,
                axis.X * axis.Z * OneMinCosT - axis.Y * sinT,
                axis.Y * axis.Z * OneMinCosT + axis.X * sinT,
                aSq.Z * (aSq.X + aSq.Y) * cosT,
                0.0,
                0.0, 0.0, 0.0, 1.0);
        }

        public static Matrix44 CreateLookAt(Vec3 eye, Vec3 target, Vec3 up)
        {
            // adapted from GLU

            Vec3 forward = (target - eye).Unit;
            Vec3 sideways, upward;
            if (!forward.IsEquivalent(up) && !forward.IsEquivalent(-up))
            {
                sideways = forward.Cross(up).Unit;
            }
            else if (!up.IsEquivalent(Vec3.XAxis))
            {
                sideways = Vec3.XAxis;
            }
            else
            {
                sideways = Vec3.ZAxis;
            }
            upward = sideways.Cross(forward);

            return new Matrix44(
                sideways.X, upward.X, -forward.X, eye.X,
                sideways.Y, upward.Y, -forward.Y, eye.Y,
                sideways.Z, upward.Z, -forward.Z, eye.Z,
                0.0, 0.0, 0.0, 1.0);
        }

    }
}
